/*Includes ----------------------------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "factorypid.h"
#include "time.h"
#include "hardiic.h"
#include "REG.h"
#include "motor.h"
#include "sger.h"
#include "usart.h"

extern float Kip, Kid;
//extern u8  USART_RX_BUF[USART_REC_LEN];
unsigned char chrTemp[30] = {0};
unsigned char str[100] = {0};
float Angle[3] = {0};
float angle = 0.0;


MAKE_FUNC_PID_S(Stable,float,-100,100);
//MAKE_FUNC_PID_S(BaA,float,-100,100);
/*Functions ---------------------------------------------------------------------------------------*/


short CharToShort(unsigned char cData[])
{
	return ((short)cData[1]<<8)|cData[0];
}

/****************************************** 定时器中断 *********************************************/

/**
  * @brief  TIM1更新中断服务函数,中断源如不符合需自行修改
  * @param  无
  * @retval 无
  */
void TIM1_UP_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源
	{
		I2C_ReadByte(0x50<<1, AX, 26, chrTemp);
		
		Angle[0] = CharToShort(&chrTemp[18])/32768.0*180;
		Angle[1] = CharToShort(&chrTemp[20])/32768.0*180;
		Angle[2] = CharToShort(&chrTemp[22])/32768.0*180;
		angle = Stable(Angle[1],-0.02,0,0.055,0);
//		if(angle < -0.01 || angle > 0.01){
			s_Ger_Control(angle,1);
//		}
		TIM_ClearITPendingBit(TIM1, TIM_IT_Update); //清除TIMx的中断待处理位:TIM 中断源
	}
}
